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IV 


ABSTRACT 


Control  and  Navigation  logic  was  developed  for  a  3-Degree  of  Freedom  Surf-Zone  Robot  to 
assist  in  the  identification  and  characterization  of  platform  parameters  for  use  in  the  Shuey  Dy¬ 
namic  Model.  These  parameters  ineluded,  primarily  platform  rotational  inertia  and  wheel  slip. 
Data  was  eolleeted  in  various  track  scenarios  including  benign  flat  terrain  and  more  eomplieated 
beach  runs.  Track  lengths  spanned  short  straight  paths  of  no  more  than  10  meters  to  full-run 
point-to-point  autonomous  navigation  paths  of  up  to  80  meters.  The  longer  runs  ineluded  turns 
of  up  to  180  degrees  and  terrain  inelines  of  2  degree  or  less.  As  expeeted  the  Shuey  model 
proved  reliable  for  short  runs  of  no  more  than  10  meters.  For  long  length  runs  in  the  beaeh 
environment  the  Dynamie  Model  diverged  quiekly.  This  is  attributed  to,  primarily,  wheel  slip 
eonditions  and  the  faet  that  the  Shuey  Model  is  open  loop.  Motor  current  was  monitored  under 
load  eonditions  to  identify  wheel  slip  and  simple  algorithms  were  implemented  to  aeeount  for 
this  with  little  sueeess.  However,  elosed  loop  heading  input  resulted  in  signifieant  improvement 
to  the  model. 
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CHAPTER  1: 

Introduction 


The  crucial  role  unmanned  vehicles  play  in  both  civilian  and  military  fields  is  becoming  diffi¬ 
cult  to  dispute.  These  platforms  are  appealing  because  of  their  amplified  on  station  time,  sensor 
capabilities  and  the  removal  of  human  risk.  Extensive  work  and  successful  developments  have 
been  made  in  both  unmanned  land  and  sea  platforms,  however,  little  work  has  been  done  to 
develop  a  robot  to  operate  in  and  transition  between  the  two  environments.  The  combination 
of  these  two  domains  is  referred  to  as  the  littoral  region,  or  Surf-zone,  which  encompasses  but 
is  not  limited  to  missions  like  surveillance,  environmental  monitoring,  intelligence,  reconnais¬ 
sance,  and  mine  clearance. 

This  region  has  not  been  extensively  explored  or  well  developed  for  good  reason.  The  Surf-zone 
requires  a  platform  able  to  adapt  to  a  variety  of  environmental  and  dynamic  factors  which  are 
dramatically  different  between  platforms  operating  exclusively  on  land  or  in  the  sea.  Several 
prototype  mechanical  platforms  have  been  developed  utilizing  biologically  inspired  locomotion 
to  traverse  the  Surf- zone. 


1.1  The  Surf-Zone  Project 

Prior  work  on  the  surf-zone  project  included  a  negatively  buoyant  Foster  Miller  Lemming 

TM 

tracked  platform  [1]  and  several  Wheg  based  platforms,  AGBOT  [2]  and  MONTe  [3]. 

Collaboration  with  Case  Western  produced  multiple  prototypes  and  programs  in  attempts  to 
meet  the  demands  of  this  complicated  dual  environment  [4].  Details  and  a  more  comprehensive 
history  of  the  Surf-zone  collaboration  can  be  found  in  the  Shuey  Thesis.  Shuey  focused  on 
creating  a  model  for  the  movement  of  a  four  wheeled  surf-zone  robot  [5]. 

The  sole  input  to  this  open  loop  model  was  time  stamped  motor  shaft  encoder  data.  Encoder 
data  sent  into  the  model  produced  time  stamped  position  X^Y  and  heading  (j)  as  output  seen  in 
Figure  1.1.  The  figure  shows  three  tracks.  Red  is  ground  truth  as  identified  by  VICON  data. 
Green  is  the  kinematic  physical  model  and  blue  is  the  dynamical  model  result. 
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Position  as  a  Function  ofTime 


X[m] 

Figure  1.1:  Shuey,  Kinematic  and  Dynamic  Model  for  a  3  degree  of  freedom  robot.  Red  is 
ground  truth.  Green  is  Kinematic  and  Blue  is  Dynamic. 


1.2  Objectives 

This  thesis  supports  Surf- zone  research  by  creating  a  new  platform  and  employing  it  to  test  the 
validity  of  the  Shuey  model  [5].  We  address  the  following  questions: 

•  Is  there  a  mobile  and  autonomous  platform  that  can  me  modeled  accurately  to  predict 
performance  and  thus  inform  the  design  process? 

•  What  are  the  key  considerations  for  autonomous  control  design? 

The  scope  of  research  and  model  design  was  limited  to  the  beach,  defined  as  the  waterline  and 
inland  for  a  distance  of  100  meters.  Operation  in  the  waterborne  environment  is  excluded.  The 
robot  was  modeled  with  three  degrees  of  freedom:  two  perpendicular  direction  coordinates  and 
one  orientation  angle. 
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CHAPTER  2: 
The  Robot 


The  Surf-zone  robot  is  designed  to  be  durable,  portable,  multi-mission  capable  and  easily  man¬ 
ufactured.  Our  design,  similar  to  previous  Surf- zone  projects,  takes  ideas  and  concepts  from 
nature  to  develop  solutions  to  the  mobility  problem.  The  design  of  our  platform,  a  Durable  Au¬ 
tonomous  Robotic  Crustacean  (DARc),  was  inspired  by  a  crab’s  mobility  and  will  later  include 
a  lobster  like  tail  for  stability  on  the  beach. 


2.1  Robot  Components 

DARc  is  designed  to  be  modular  as  discussed  in  the  following  sections. 

2.1.1  Platform 

The  skeleton,  in  Figure  2.1a  holds  a  pelican  case.  Figure  2.1b,  containing  mission  specific 
sensors,  motors,  batteries  and  micro-controller,  that  will  be  discussed  in  Section  2.1.3.  This  al¬ 
lows  for  quick  transition  between  operations  requiring  dissimilar  sensors  or  programs.  Imagine 
switching  between  a  surveillance  mission  and  a  mine  hunting  assignment.  Each  utilizes  differ¬ 
ent  sensor  and  communication  suites.  Also,  each  has  a  different  likelihood  of  return.  DARc’s 
adaptability  allows  unnecessary  but  valuable  components  to  be  removed  when  there  is  a  higher 
mission  risk. 

The  Whegs,  as  seen  in  Figure  2.2,  were  produced  using  a  three-dimensional  printer.  This  fab¬ 
rication  technique  contributes  to  the  ease  and  speed  of  manufacturing,  especially  in  isolated 
regions.  A  unit  in  the  field  need  simply  hit  the  print  button  and  assemble  the  Wheg.  DARc’s 
design  will  later  include  a  lobster  tail  similar  to  the  one  created  for  the  201 1  Slatt  thesis  [6]. 

2.1.2  Hardware 

For  autonomy  we  used  a  BL2600  single  board  computer,  which  incorporates  a  Rabbit  3000 
microprocessor,  digital  10,  flash  and  RAM  memory  and  both  RS-232  and  RS485  serial  ports 
as  seen  if  Figure  2.3.  These  features  allow  smooth  integration  with  the  sensors  discussed  in 
Section  2.1.3  and  the  data  storage  discussed  in  Section  3.2. 
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(a)  Skeleton  (b)  Pelican  Case 


Figure  2.1:  Hardware  components  are  enclosed  in  the  pelican  case  (Figure  2.  lb)  to  protect  them 
from  the  elements.  The  pelican  case  is  then  placed  in  the  Skeleton(Figure  2.1a).  The  case  holds 
a  single  board  computer,  power,  and  sensors.  The  skeleton  contains  the  essential  components 
not  expected  to  change  between  missions.  This  includes  the  motor-controllers,  the  Whegs,  and 
the  aluminum  structure  that  houses  the  pelican  case. 


Figure  2.2:  DARc  Wheg  design  printed  by  a  three-dimensional  printer. 


2.1.3  Sensors 

The  Garmin  GPS  unit  (Figure  2.4b),  is  used  to  assist  in  way  point  navigation.  It  is  capable  of 
tracking  up  to  twelve  satellites  and  has  differential  GPS  capability  providing  accuracy  within 
three  meters  [7].  The  design  is  well  suited  for  DARc  because  it  is  waterproof  and  rugged. 
Position  data  is  provided  via  an  RS232  connection  in  a  GPS  Fix  Data  Format  string,  including 
but  not  limited  to,  time  of  fix,  position  details,  fix  accuracy  and  number  of  satellites  used  for  the 
fix. 

To  measure  heading,  we  used  a  Honeywell  HMR3000  Magnetic  Compass  seen  in  Figure  2.4a. 
This  selection  was  made  because  of  the  Honeywell’s  durability,  accuracy,  availability  and  ease 
of  use.  Parsing  of  both  the  GPS  and  heading  strings  are  discussed  in  Section  3.1.2. 
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E  and  F 
serial  ports 


DC  to  DC  switching 
power  supply 


DC  to  DC  switching 
power  supply 


DC  to  DC 
switching 
power 
supply 


Quarter  for 
reference 


BL2600  Wolf  single 
board  computer 


(a)  Upper  Level  Components 


DC  brush  motor  servo 
controllers 


Lithium- 
ion  16  volt 
battery 


Quarter  for 
reference 


Lithium-ion  16  volt 
batteries  in  series 


(b)  Lower  Level  Components 

Figure  2.3:  The  peliean  ease  houses  two  levels  of  eomponents  as  identified  above. 
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(b)  Garmin  GPS  1 8 


(a)  Honeywell  Magnetic  Compass 

Figure  2.4:  DARc  employed  two  external  sensors;  a  Honeywell  magnetic  compass,  Figure  2.4a, 
and  a  Garmin  GPS  18,  Figure  2.4b. 
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CHAPTER  3: 
Algorithm  and  Control 


Before  modeling  and  performance  assessments  could  be  conducted,  we  needed  to  create  basic 
control  and  navigation  code  and  achieve  autonomy.  Dynamic  C  is  a  non-standard  programming 
language  designed  to  operate  the  Rabbit  micro-controller  and  has  been  extensively  used  in  each 
of  the  previous  Naval  Postgraduate  School  (NPS)  surf  zone  projects. 

3.1  Navigation 

Figure  3. 1  is  a  flowchart  of  DARc’s  basic  costatements  and  functions.  Costatements,  or  costates, 
are  Dynamic  C  additions  to  the  C  language  which  simplify  cooperative  mulit- tasking.  Costates 
are  usually  nested  in  a  while  loop  in  order  of  priority.  The  program  continues  to  cycle  through 
them  as  illustrated  in  Figure  3.2.  Each  costate  may  contain  a  "pause"  where  the  program  must 
wait  for  an  event,  a  specified  passage  of  time  or  a  set  of  information.  During  a  pause,  the 
program  will  leave  the  costate  and  continue  to  the  next  costate  in  the  loop  and  return  to  that 
position  when  the  necessary  event  has  occurred. 

Within  each  costatement  is  an  ordered  list  of  tasks  or  functions  grouped  together  in  support  of 
a  program  operation  like  the  navigation  costate  in  Figure  3.1.  The  functions  take  in  data  from 
outside  sensors  and  calculate  the  required  parameters  including  but  not  limited  to  position, 
heading,  desired  heading  and  distance.  Priority  for  each  statement  is  driven  by  operational 
requirements.  Before  entering  the  loop  of  costates,  the  program  initializes  the  board,  serial 
ports,  and  creates  a  file  for  data  storage  as  seen  in  Appendix  A. 

3.1.1  Sensor  Data 

Data  from  the  compass  and  GPS  are  sent  to  the  BL2600  through  the  F  and  E  serial  ports.  Each 
produces  a  data  string  that  uses  the  format  from  the  National  Marine  Electronics  Association 
(NMEA),  a  US  organization  that  sets  communications  standards  for  marine  electronics.  An 
example  of  each  string  with  the  respective  explanations  are  listed  in  Tables  3.1  and  3.2. 

String  parsing  for  both  the  GPS  and  Heading  functions  is  very  similar.  In  each  case  characters 
are  individually  evaluated  based  on  the  NMEA  standards.  This  information,  attained  in  the 
compass  and  GPS  costates,  is  then  sent  to  the  navigation  costate  where  desired  heading  and 
distance  are  calculated. 
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CompassString  H 


Compass 

Costate 

1 

GPS 

Costate 

Navigation 
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^  i 

t 

Heading 
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.  Desired  Heading 

Ij 

Encoder  Counts 


Motor  Controller 
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Written  to 
BL2600 
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Figure  3.1:  The  eode  has  three  major  eomponents;  initialization,  sensor  reading  and  navigation. 
Blue  boxes  are  the  funetions  and  eostates.  Green  rombi  represent  the  outside  sensors  and  the 
purple  eylinder  is  the  plant.  Plain  text  is  information  passed  between  funetions. 


3.1.2  Position  and  Heading  Calculations 

Compared  to  the  radius  of  the  earth,  the  distanees  between  our  way  points  were  relatively  elose 
together.  This  allowed  for  a  flat  earth  approximation.  The  navigation  funetion,  found  in  Figure 
3.1,  is  where  distanee  and  heading  are  ealeulated.  First,  we  eonverted  the  GPS  coordinate  into 
Cartesian  space  where  West  and  South  were  treated  as  the  negative  parts  of  the  X  and  Y  plane 
shown  in  Figure  3.4.  Equation  3.1  calculates  the  distance,  d,  between  two  way  points  using  the 
distances  in  the  x  and  y  planes.  Equation  3.2  calculates  the  heading,  h,  to  the  desired  way  point. 
Eigure  3.4  visualizes  the  geometry  and  assumptions  made  for  these  calculations. 

d  =  a/  +  Ay2  (3.1) 

h  =  IS0°  —  arctani—)  (3.2) 

Ax 
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String 

Meaning 

GGA 

Global  Positioning  System  Fix  Data 

123456 

Fix  taken  at  12:34:56  UTC 

3635.7058,N 

Latitude  36  deg  35.7058’  N 

12152.4939,W 

Longitude  121  deg  52.4939’  E 

2 

Fix  quality:  0  =  invalid 

1  =  Non  Differential  fix 

2  =  Differential  fix 

5 

Number  of  satellites  being  tracked 

Table  3.1:  The  NMEA  GGA  string  format  meaning  for  GARMIN  18  using  the  example  string 
'$GEGGA,  123456, 3635.7058,  A,  12152.4939,  IT,  2, 5'. 


String 

Meaning 

301 

30l°True 

Table  3.2:  Honeywell  Compass  format  using  the  example  string  "$PTNTHPR,  301’. 


Figure  3.3  is  a  sample  of  the  navigation  code  for  calculating  distance  and  heading.  Latitude 
and  Longitude  are  converted  to  Cartesian  coordinates  and  then  used  to  determine  distance  and 
heading  to  the  next  way  point. 

3.1.3  PID  Control 

While  a  Wheg  design  is  excellent  for  traversing  in  the  surf  zone  environment  [6],  the  shape  tends 
to  exacerbate  the  need  for  course  correction.  In  order  to  steer  a  steady  course  we  implemented 
proportional-integral-derivative(PID)  control.  Figure  3.5  is  a  diagram  of  this  feedback  loop  and 
Equation  3.1.3  is  the  formula  which  calculates  the  signal,  u(t),  sent  to  the  plant.  Kp,  Ti  and 
Tj  are  the  proportional,  integral  and  derivative  gain  values,  e{t)  is  the  error  as  a  function  of 
time,  and  s  is  time  in  seconds.  The  experiment  to  obtain  the  compensator  values  is  discussed  in 
Chapter  5. 


u{t)  =Kp{l  + 


1 


Tds)e{t) 


(3.3) 


3.2  Data  Extraction 

Once  able  to  drive  a  reliable  path,  our  focus  turned  to  pulling  information  for  later  use.  Data  Ex¬ 
traction  can  be  conducted  in  a  variety  of  ways.  Previous  thesis  work  kept  the  platform  tethered 
to  a  laptop  computer  and  extracted  data  by  printing  directly  to  the  screen.  This  method  had  two 
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disadvantages.  First,  tethering  proves  difficult  when  attempting  to  make  visual  observations, 
prevent  cables  from  getting  caught  up  in  the  wheels,  and  executing  robust  paths.  Secondly,  the 
print  statements  necessary  for  the  tether  system  slows  the  frequency  of  data  steps  and  reduces 
model  accuracy.  A  wireless  connection  is  also  a  viable  solution,  however,  more  coding  and 
components  would  be  needed  for  the  platform. 

Instead  of  a  tether  or  using  a  wireless  connection,  we  took  advantage  of  the  file  library  in 
dynamic  C  and  developed  code  to  extract  and  store  data  which  could  later  be  retrieved  when  the 
robot  was  connected  again  in  the  dry  and  stationary  lab  environment.  The  developed  extraction 
code  is  found  in  Appendix  B  .  For  each  step  we  recorded  the  time,  encoder  counts,  current 
readings  for  each  motor,  latitude,  longitude  and  the  heading.  After  extraction,  analysis  was 
conducted,  as  will  be  discussed  in  Chapter  5. 
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f  ‘  Loop  2"^  Loop  3^^  Loop 


Figure  3.2:  Here  are  three  iterations  of  an  example  program  with  costates.  The  first  goes  through 
each  statement  completely  and  returns  to  the  top  of  the  program.  In  the  second  loop,  the  program 
finds  a  pause  partway  through  statement  one  and  continues  to  the  next  costate.  On  the  third 
loop,  the  program  goes  back  to  the  pause  point,  finishes  costate  1  from  that  point  on  and  then 
continues  to  costate  2. 
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currentjat  =  (double)current_pos.lat_degrees+  (current_pos.lat_minutes/ 60.0); 
if  (current_pos.lat_direction==  'S') 

currentjat  =  0  -  currentjat;  //negative  values  in  Southern  hemisphere 

current  Jon  =  (double)current_pos.lon_degrees  +  (current_pos.lon_minutes/  60.0); 
if  (current_pos.lon_direction==  'W') 

currentjon  =  0  -  current  Jon;  //negative  values  for  West  hemisphere 

//  Desired  Location 
wypt_lat  =  desired_WP.Iat; 
wypt_lon  =  desired_WP.Ion; 

err_distance=  distance(currentJon,current_lat,  wyptjon,  wypt_lat);  //Error  distance  between  goal  and  starting  position 

desired_heading=(180.0/M_PI)'’  normalize2PI(M_PI/2-  headingTo(current Jon, currentjat,  wyptjon,  wyptjat)); 


Figure  3.3:  A  sample  from  my  Dynamie  C  eode  found  in  appendix  A.  Latitude  and  Longitude 
are  eonverted  to  Cartesian  eoordinates  and  then  used  to  determine  distanee  and  heading  to  the 
next  way  point. 
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Figure  3.4:  Distance  and  heading  are  calculated  using  cartesian  coordinates  and  basic  geometry 
principles  as  shown  here.  Ax  is  the  change  in  latitude  and  Ay  is  the  change  in  longitude  from 
one  way  point  to  another.  The  heading  and  distance  are  continually  calculated  which  influences 
motor-controller  speeds.  DAR-C  operates  in  the  Northern  hemisphere  therefore  true  desired 
heading  is  obtained  by  subtracting  the  cartesian  heading  from  180°.  In  this  figure  the  true 
heading  is  roughly  180°  —  (—45°)  =  225°. 
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Figure  3.5:  PID  feedback  loop:  A  desired  heading  is  sent  to  the  controller  that  calculates  error 
by  comparing  it  to  the  desired  heading.  The  lower  illustration  details  the  calculations  of  the 
compensator. 
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CHAPTER  4: 
Theory 


The  methods  we  used  for  developing  a  robotie  simulation  eentered  on  fundamental  physies 
theories  ineluding  energy  ealeulations,  inertia  tensors,  traditional  meehanies  and  Lagrangian 
meehanies.  The  first  set  of  ealeulations  and  programming  originates  from  the  Shuey  Thesis. 
This  ehapter  will  briefly  explain  that  work. 

4.1  Shuey  Model 

The  Shuey  model  is  eomposed  of  two  parts;  a  kinematic  model  and  a  dynamic  model.  The  first 
uses  basic  wheel  rotation  and  movement  principals  while  the  second  uses  Lagrangian  mechanics 
to  develop  a  more  complex  and  accurate  model.  For  each  case,  the  following  assumptions  were 
made: 


•  The  platform  is  modeled  as  a  two  wheeled  differentially  steered  robot  rather  than  a  four 
wheel  skid-steered  robot. 

-  no  lateral  slippage 

•  The  robot  operates  with  three  degrees  of  freedom(two  for  position  and  one  for  orienta¬ 
tion). 

-  no  changes  in  elevation 

4.1.1  Kinematic  Model 

The  first  model  is  a  simple  kinematic  description  of  the  robot’s  movement.  The  dead  reckoning 
portion  calculates  the  average  velocity  of  the  two  wheels,  using  encoder  counts,  the  times  step 
and  wheel  radius.  The  model  iterates  at  such  a  slight  time  change  that  at  each  step  we  are  able 
to  get  a  very  small  account  of  forward  and  angular  velocity.  Equation  4. 1  determines  forward 
speed,  i,  of  the  robot  in  it’s  own  frame  of  reference,  where  r  is  the  radius  of  the  wheel  and  (j)  is 
the  wheel  velocity  for  the  left  or  right  wheel. 


X  =  r — - — 


(4.1) 


Equations  4.2,  4.3,  and  4.4  are  the  final  results  from  the  Shuey  kinematic  model  where  A0  is 
the  platform’s  change  in  heading,  AIT  is  the  change  in  number  of  revolutions  for  a  right  or  left 
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wheel,  and  d  is  the  axle  length.  The  change  in  heading,  A0  is  summed  to  compute  the  heading 
at  each  time  step.  The  last  equation  provides  the  platform’s  position  in  the  global  frame,  Xw 
and  Yw- 


AWl-AWr 

Ad  =  iTtr - - - . 

a 

(4.2) 

AXvr  =  ;rrcos(0)(AWL-hAWR) 

(4.3) 

AYw  =  ;rrsin(0)(AWz.  +  AiyR). 

(4.4) 

While  the  kinematic  model  is  simple  to  create  and  understand,  it  does  not  accurately  account  for 
acting  forces.  A  more  complex  set  of  equations  is  required  to  more  precisely  capture  motion. 

4.1.2  Dynamic  Model 

The  dynamic  equations,  developed  using  Lagrangian  mechanics,  yielded  Equation  4.5  then 
expanded  in  Equation  4.6.  A  new  force,  torque  and 


L  —  Tg-Y  T-wl  +  TwR  —  U , 


(4.5) 


L  =  -mx^  +  -my2  -)- 


(4.6) 


Next,  using  the  Euler-Eagrange  equation  and  two  orders  of  integration  the  equations  of  motion 
were  produced,  found  in  Equations  4.7,  4.8,  and  4.9.  All  three  equations  require  current  veloc¬ 
ities,  forces  and  torque.  These  values  change  throughout  the  run,  however,  for  simplicity  the 
model  treats  them  as  a  constant  at  each  time  step.  At  the  next  time  step,  new  velocity,  force  and 
torque  values  are  calculated  with  equations  4.1,  4.1 1  and  4.12  then  to  the  equations  of  motion. 


/  N  1  2 

x{t)  =  - - 1  +XQt+XQ 

2  m 


(4.7) 


1  F 

y{t)  =  t^+yo^+yo 

2  m 


(4.8) 
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e(<) 


(4.9) 


+  00^  +  00 


The  above  equations  are  also  governed  by  constants  including  mass,  dimensions  and  inertia. 
Table  ??;  DARc’s  dimensions  which  contributed  to  the  inertia,  force,  torque  and  position  equa¬ 
tions.  We  calculated  body  inertia  as  l.93kg'm^  and  wheel  inertia  as  OAOlkg'm^. 


Parameter 

Measurement 

Length  (front  to  back) 

.445  m 

Width  (side  to  side) 

.458  m 

Height 

.188  m 

Wheel  Separation 

.285  m 

Wheel  to  COM  separation 

.316 

Wheel  Radius  Outer/Inner 

.127  m  .085cm 

Wheel  Mass 

.7055  kg 

Body  Mass 

13.542  kg 

Table  4.1:  DAR-C  Dimensions 


The  force  on  each  wheel.  Equation  4.10  is  calculated  from  the  wheel’s  angular  velocity,  inertia 
and  density,  p.  These  are  then  combined  to  find  forward  force,  Fx,  and  torque,  T. 


Fl 


F)rIw 

P 


(4.10) 


Fx  =  FlF  Fr 


(4.11) 


^=\{Fr-Fl)  (4.12) 

The  Shuey  model  assumes  no  slip,  therefor,  lateral  force,  Fy,  is  considered  zero.  These  calcula¬ 
tions  are  then  submitted  to  MATLAB’s  ode45  solver  to  produce  position  and  orientation  in  the 
world  frame. 
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CHAPTER  5: 
Experiment  and  Results 


There  were  two  experimental  portions  for  this  thesis.  First,  we  designed  a  test  to  obtain  eontrol 
eoeffieients.  Seeond,  data  was  eolleeted  by  navigating  between  waypoints.  The  information 
was  then  used  to  test  the  Shuey  model. 

5.1  Control  Coefficient  Experiments 

As  diseussed  in  Seetion  3.1.3,  plant  signals  were  determined  using  a  PID  eontroller.  We  em¬ 
ployed  the  Ziegler-Niehols  method  to  ealeulate  eaeh  gain  value.  First  we  implemented  a  eon¬ 
troller  with  only  proportional  gain(^p).  We  then  increased  the  value  for  Kp  until  we  achieved 
steady  oscillation  as  seen  in  Figure  5.1.  This  value  became  our  critical  gain,  Kq,  and  the  oscil¬ 
lation  period  for  this  run  became  Tq. 


Kp  =  1 .45 


Figure  5.1:  Here,  the  robot  was  ordered  to  steer  a  steady  course  of  190°  using  proportional 
control  where  Kp  =  1.45.  Average  frequency  is  4.6  seconds.  We  used  these  two  values  to 
calculate  our  PID  gain  values  by  the  Ziegler-Niehols  method. 
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These  values  were  then  used  to  calculate  PID  gains  with  the  Ziegler-Nichols  equations  found  in 
Table  5.1.  Equation  5.1  is  the  final  PID  controller  equation  with  the  calculated  gains  from  the 
experiment  and  the  equations  in  Table  5.1. 


Controller 

Kp 

Ti 

Td 

P 

Ko 

2 

PI 

^0 

1 

2.2 

i.2ro 

PID 

Ko 

1.7 

To 

2 

To 

8 

Table  5.1:  Ziegler-Nichols  Parameters 


M(t)  =  .85(1  +  — +  .635)e(t)  (5.1) 

2.55 

5.2  Evaluation  of  Shuey  Model  Using  DARc  and  Beach  En¬ 
vironment 

After  constructing  DARc  and  developing  reliable  control  we  were  able  to  test  the  Shuey  model 
with  the  new  platform.  There  are  several  important  differences  between  the  Shuey  tests  and 
ours.  The  first  testing  environment  was  in  a  controlled  lab  space  with  a  smooth,  flat,  cement 
surface  where  the  second  was  in  the  beach  environment  with  diverse  surface  composition  and 
varied  elevation  as  seen  in  Figure  5.2. 

The  platforms  were  also  significantly  dissimilar  with  the  first,  Sandie,  having  round  rubber 
wheels  and  employing  deterministic  path  execution.  Contrarily,  DARc  employed  polycarbon 
WHEGS  and  autonomous  navigation. 

5.2.1  Initial  Results 

The  experiment  was  conducted  at  the  Del  Monte  Beach  in  Monterey  California  on  terrain  reach¬ 
ing  inclines  up  to  two  degrees  and  terrain  type  ranging  from  hard  packed  to  medium  packed 
sand.  DARc  was  ordered  to  conduct  navigation  between  way  points  while  simultaneously  stor- 


20 


(a)  DARc  at  Del  Monte  Beach  in  Monterey,  Califor-  i  i  . 

^  (b)  Sandie  at  the  NFS  Vicon  labratory. 

nia. 

Figure  5.2:  Above  are  the  two  trial  platforms  in  their  respective  testing  environments.  DARc, 
Figure  5.2a,  is  on  the  beach  terrain  comprised  of  varying  surface  composition  and  inclines. 
Sandie,  Figure  5.2b,  is  observed  from  above  in  the  indoor  NFS  VICON  laboratory. 


ing  experiment  data  in  RAM  for  later  extraction.  Figure  5.3  shows  the  results  from  one  of  our 
beach  runs.  Note  that  the  dynamic  model  predicts  position  well  within  the  first  ten  to  twenty  me¬ 
ters,  however,  drifts  significantly  to  the  right  roughly  half  way  through  the  run.  It  was  observed 
that  during  this  test  the  left  wheel  experienced  slip.  The  model  assumes  a  no  slip  environment, 
as  it  has  no  way  to  account  for  loss  of  traction,  thus  while  the  left  wheel  was  spinning  the  model 
interpreted  this  as  a  right  turn. 

All  runs  conducted  yielded  similar  results  and  were  unable  to  account  for  slip  resulting  in  sig¬ 
nificant  deviation  from  platform’s  true  heading  and  notable  error  in  final  estimated  position. 
Figure  5.4  is  a  beach  run  were  the  platform  navigated  between  three  way  points.  Significant 
slip  was  experienced  resulting  in  a  poor  model  estimate  of  platform  location  and  orientation. 


5.2.2  Accounting  for  Slip 

Original  testing  of  the  Shuey  model  was  conducted  in  an  environment  where  a  no  slip  assump¬ 
tion  had  much  less  effect  on  model  accuracy.  The  change  in  platform  type,  going  from  a  round 
wheel  to  a  WHEG,  and  the  varrying  and  rugged  terrain  made  this  assumption  impossible  when 
looking  to  accurately  model  DARc’s  motion. 
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Position  as  a  Function  of  Time 


Figure  5.3:  This  run  was  conducted  at  the  beach  where  DARc  navigated  between  two  way 
points.  Midway  through  the  run  the  platform  experienced  slip  in  the  left  wheel.  The  Shuey 
model  has  no  way  of  accounting  for  slip  and  thus  the  produced  path  deviated  significantly  from 
real  world  data. 


Current  and  Slip 

To  identify  slip  we  took  a  closer  look  at  the  motor  currents  [8].  We  first  identified  time  steps 
with  large  differences  in  current  between  the  two  motors  as  seen  in  Figure  5.5.  Next,  these  time 
steps  were  evaluated  such  that  wheels  experiencing  low  current  were  identified  as  being  in  a 
slip  condition,  as  seen  if  Figure  5.7.  The  slip  threshold  for  both  the  current  difference  and  each 
individual  current  was  determined  using  a  best  fit  method  for  the  multiple  beach  runs.  Assumed 
speed  for  identified  slip  time  steps  was  reduced  for  the  slipping  wheel.  Figure  5.8  compares  the 
original  model  to  the  modified  slip  reduction  model  for  two  runs.  The  calculated  orientation 
and  position  is  improved,  however,  this  modification  did  not  completely  correct  the  model. 

Heading  Modification 

Next  we  employed  real  data  and  closed  the  dynamic  model  loop  by  periodically  updating  the 
heading.  Results  from  the  modification  are  found  in  Figure  5.9.  These  results  encourage  future 
work  to  focus  on  preventing  significant  orientation  changes  during  slip  conditions. 
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Figure  5.4:  This  run  was  conducted  at  the  beach  where  DARc  navigated  between  three  way 
points.  Halfway  to  the  second  waypoint  the  platform  experienced  left  wheel  slip.  The  Shuey 
model  interpreted  this  as  a  right  turn  which  caused  significant  error  in  the  model  orientation. 
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Current  Difference 


Figure  5.5:  This  graph  identifies  large  differences  between  motor  currents.  We  used  a  best  fit 
method  for  multiple  beach  runs  to  determine  the  threshold  for  large  current  differences  indi¬ 
cating  slip.  Time  steps  where  the  current  difference  was  above  the  threshold  is  then  examined 
more  closely  for  low  currents  to  determine  slip(as  illustrated  in  the  decision  chart  in  Figure  5.6) 


Figure  5.6:  Decision  chart  for  identifying  slip:  The  thresholds  for  determining  a  high  current 
difference  and  a  motor  with  low  current  was  determined  using  a  best  fit  method. 
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Figure  5.7:  The  run  shown  in  Figure  5.3  was  evaluated  for  time  steps  experiencing  slip  using 
the  decision  chart  in  Figure  5.6.  Red  lines  indicate  a  time  step  where  the  left  wheel  experienced 
slip  and  green  lines  indicate  the  same  for  the  right  wheel.  Velocities  for  time  steps  and  wheels 
experiencing  slip  were  then  reduced. 
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Shuev  Model 


Slip  Reduction  Model 


Figure  5.8:  Two  runs  assessed  using  the  original  Shuey  model,  left,  and  the  new  slip  reduetion 
model,  right. 
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Figure  5.9:  Two  runs  assesed  using  the  original  Shuey  model,  left,  and  the  elosed  loop  model, 
right. 
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CHAPTER  6: 
Conclusion 


While  the  Shuey  model  produced  a  fairly  accurate  account  of  the  robot’s  real  world  position, 
ignoring  skid  and  slip  prevented  the  model  from  precisely  calculating  heading  and  position. 

6.1  Shuey  Model  Assesment 

The  original  Shuey  model  is  best  suited  for  situations  where  little  to  no  slip  exists.  This  would 
include  platforms  with  round  wheels,  especially  truly  deferentially  steered  platforms.  Also, 
environments  with  flat,  smooth  surfaces  and  no  elevation  change  produce  more  accurate  model 
results.  The  surf  zone  platforms  and  the  environment  they  are  intended  for  meet  neither  of  these 
two  requirements,  thus  significant  work  needs  to  be  conducted  to  account  for  slip  and  skid. 

6.2  Future  Work 

Future  thesis  work  should  explore  modifying  the  model  to  reflect  the  platform  steering  method. 
Skid-steering,  as  opposed  to  differential  steering  assumed  in  the  model,  is  a  more  accurate  way 
to  describe  our  platform. 

6.2.1  Skid  and  Differentially  Steered 

Identifying  the  mode  for  steering  is  a  crucial  first  step  in  creating  an  accurate  model.  Ackerman 
steering,  skid  steering  and  differential  steering  are  three  very  common  methods  used  in  wheeled 
robotics.  Most  of  the  surf  zone  platforms  have  employed  skid  steering.  The  only  exception  is 
AGBOT  which  used  ackeman  steering,  a  technique  where  wheels  are  able  to  rotate  relative  the 
robot  body.  The  easiest  example  of  a  differentially  steered  platform  to  visualize  is  the  wheel 
chair.  The  assumptions  from  the  Shuey  thesis  allowed  for  this  model  to  come  very  close  to 
describing  the  robot  path  accurately.  Specifically,  because  Shuey  assumed  no  slip  and  no  lateral 
force,  the  model  produced  was  very  close.  Skid  steering  is  a  more  accurate  way  to  describe 
our  platform.  Visualize  a  tank.  Unlike  your  car,  to  turn  the  wheels  must  either  slip  or  skid  as 
the  heading  changes.  Moving  from  a  concrete  to  a  gravel  or  sand  environment  dramatically 
increases  the  need  to  account  for  slip  and  friction.  While  differential  steering  is  more  accurate 
for  less  slip  prone  environments  like  concrete,  for  loose  gravel,  like  sand,  where  slipping  is  very 
common  we  need  to  account  for  slippage.  The  surf  zone  is  a  slip  prone  sand  environment  and 
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thus  sliding  and  skidding  must  be  taken  into  account  by  more  accurately  modeling  the  steering 
method. 

The  relation  of  slip  and  current  should  be  examined  more  thoroughly.  Testing  should  be  con¬ 
ducted  comparing  ordered  speed,  expected  speed,  actual  speed,  and  current. 

While  autonomous  platforms  have  been  developed  and  increasingly  utilized  in  exclusively  land, 
sea  or  air  environments,  there  has  yet  to  be  a  defense  design  employed  in  the  littoral  region. 
Proficiency  in  the  surf  zone  environment  is  crucial  to  the  Navy’s  ability  to  transition  between 
land  and  sea  while  maintaining  command  and  control  in  both  regions.  This  platform  would  be 
excellent  for  mine  clearance  and  detection  or  removal  of  other  integrated  defenses.  Having  an 
integrated  sensor  suite  would  be  invaluable  in  mission  planning  and  intelligence  gathering.  In 
order  to  reap  the  benefits  that  autonomous  platforms  have  yielded  in  other  environments  it  is 
essential  that  accurate  models  and  simulations  be  developed  and  perfected  to  inform  the  design 
process  and  accurately  predict  performance. 
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APPENDIX  A: 

Control  and  Navigation  Code 


/*  F i t zger ald.Thes i s_20 1 3  .  c 

*  Programmers:  Jessica  L.  Fitzgerald 

*  Programs  Referenced/Used:  Steve  Jacob’s  ’2  SERVO  POSITION  &  AD  VALUE. C’ 

*  and  PC4015  Winter  2012’s  BENDER20 1 2V 1 20 . C 

*  Supervisor:  Prof  Harkins 

* 

*  Program  Description:  This  program  consists  of  a  number  of  functions  folovted 

*  by  the  mainO  file.  Thus  far  the  following  have  been  implemented: 

*  1.  Desired  speeds,  accelerations  and  predetermined  paths  can  be  sent  to 

*  and  executed  by  the  Rabit. 

*  2.  Data  is  recorded  to  the  Rabit ’s  RAM. 

*  a.  Data  can  be  extracted  using  either: 

*  i.  PRINTFILE.C  (preferred) 

*  ii.  DUMPFILE.c 

*  b.  Data  Recorded  incldudes  time(mS),  encoder  count,  current 

*  for  both  motors,  headingCfrom  compass).  Latitude,  and 

*  Longitude ( from  GPS). 

*  3.  Platform  navigates  using  sensors: 

*  a.  Steer  a  course  using  drive.Heading ()  (employs  PID  control) 

*  b.  Drive  between  Waypoints  (uses  drive.heading ()  function) 

*  4.  Program  currently  designed  to  run  between  3  waypoints  on  the  Del 

*  Monte  Beach . 

*/ 

///////////////////////////////////////////////////////////////////// 

//File  and  basic  commands  declarations 

///////////////////////////////////////////////////////////////////// 

#class  auto 
#memmap  xmem 
#use  ''FS2.LIB'' 

#define  TESTFILE  1 
#define  TESTFILE2  2 

#define  PIC.STEP 
#define  PIC.SERVQ 
#define  CINBUFSIZE  511 
#define  COUTBUFSIZE  511 
#use  nmc . lib 

#define  DEBUG. PRINT  0 
#define  DATA.PRINT  0 

#define  Velocity  30000 
#define  Acceleration  100 

#define  sixteenFootTicks  27500 
#define  oneRev  1961.5 
#define  turnRev  3675 

#define  WAIT  2  //should  be  dependant  on  speed  and  operating  method 
#define  ms2wait  12000  //  6000  =  3  12000  =  6  18000  =  9  24000  =  12 

#define  pi  3.1416 

#define  sideOfBox  oneRev*2  //  3  then  6  revs  9,  12  /  2013  1-24 


int  iterator,  inter,  slt,slt2,  MIAD ,  M2AD ,  successFlag; 
char  ch ; 

long  tO  ,  timer,  lef tPosition ,  rightPosition ,  averagePosition ,  distanceTilNext ; 


//  1  to  print ,  0  for  NO  print 
//  1  to  print ,  0  for  NO  prin 
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long  spiralln  ,  spiralOut; 

long  pivotTicks; 

long  inf ormation  [5] ; 

long  lef tVelocity  ,  rightVelocity ; 

static  char  bufferl  [10]  ; 

//Initialization  Methods 

void  robotlnit(void); 

void  read2s t at s ( VO id )  ; 

void  s e t ServoParamet er s ( VO id ) ; 

//Basic  Motor  Control  Methods 

void  setLeft Speed ( long  vel ,  long  accel); 

void  s etRight Speed ( long  vel,  long  accel); 

//Advanced  Motor  Control  Methods 

void  goStraight ( long  vel,  long  accel,  int  dir); 
void  carTurndong  vel,  long  accel,  int  dir); 
void  stop (void)  ; 

void  runLine(File  *file,  int  baseSpeed); 
void  runCircle (File  *file,  int  baseSpeed); 
void  runSpiral (void) ; 
void  makePivotCCW (void)  ; 

//Logging  Methods 

void  1 oginf ormat ion ( long  di s t anceTi INext )  ; 
void  logData(File  *f); 

///////////////////////////////////////////////////////////////////// 

//Compass  declarations 

///////////////////////////////////////////////////////////////////// 

#define  MAX.SENTENCE  100 

#define  FINBUFSIZE  255 

#define  FOUTBUFSIZE  255 

#define  COMPASS.DELAY  10 

#define  READDELAY  15 

#define  COMPASS.WAIT.TIME  1 

//set  EXT.SENSORS  to  0  running  without  BL2000  connected  to  sensors 
//set  EXT_SENS0RS  to  1  when  BL2000  has  all  sensors  connected 
#define  EXT.SENSQRS  1 

#define  HEADING.PRINT  0  //  1  to  print,  0  for  NO  print 


//function  prototype  to  communicate  on  serF  port  and  set 
int  compass(char  sentence [MAX.SENTENCE] ) ; 
void  msDelaydong  sd); 

//global  float  heading 

void  Ge tCompas sSt ring ( char  * Re t Compas sSt r ) ; 

//configuration  sentence  to  compass 

const  char  init_str[]  =  " #BAD =1 1 *7 A\ r \n " ;  //5  times  per  second 
float  curr.hdg; 

char  compass.sentence  [MAX.SENTENCE]  ; 
int  compass. error ; 

//milliseconds  to  delay  between  compass  readings,  this  was  50 
const  int  compass. delay  =  10; 
int  string. pos; 
char  input. char; 

unsigned  long  c ompas s.wai t .t ime ; 
const  int  compass. timeout  =  1; 

int  Compass. update ; 

//Montery  is  East  14  so  add  14  to  the  compass  mag  to  get  true  head 
float  magVar ; 
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float  heading ; 


///////////////////////////////////////////////////////////////////// 

//Navigation  declarations 

//You  can  implement  a  P,  PI  or  PID  controller  by  commenting  out  the 
//unused  sections. 

///////////////////////////////////////////////////////////////////// 

#define  BASE. SPEED  150 

#define  k.O  1.45 
#define  T.O  5 


//P  Controller 
#define  k.p  (.5*k_0) 
#define  k.i  0 
#define  k.d  0 
//PI  Controller 
/*#define  k.p  .45*k.O 
#define  k.i  ( 1 / ( 1 . 2* T.O ) ) 
#define  k.d  0 
//PID  Controller 
#define  k.p  .6*k.0 
#define  k.i  .5*T.0 
#define  k.d  .125*1.0  */ 


//  Proportional  Control  Parameter 
//  Integral  Control  Parameter 
//  Derivative  Control  Parameter 


//  Integral 
//  Derivative 


//  Proportional 
Control  Parameter 
Control  Parameter 


Control  Parameter 


//  Proportional  Control  Parameter 
//  Integral  Control  Parameter 
//  Derivative  Control  Parameter 


#define  PID. PRINT  0 
#define  NAV. PRINT  0 
#define  M.PI  3.14159 
#define  DISTANCE. ERROR  .00005 

//  itself  "  at '' 
waypoint  i 
this 


//  1  to  print  , 
//  1  to  print  , 


0  for  NO  print 
0  for  NO  print 


//  Error  distance  allowed  (Robot  considers 


float  desired. heading ;  //  defined  in  mainO  or  in  functions 

float  error. sum ,  previous. heading ,  previous. error ; 

float  error. distance ;  //Distance  from  current  position  to  intended  waypoint 
void  dr ive .heading ( f loat  requested. heading) ; 
float  PID. Controller (void) ; 


//  uses  flat  Earth  approximation  around  robot’s  current  position 
//  Define  polarity  for  robot,  N  hemisphere (+)  and  East  hemisphere  (+) 

const  float  esquared  =  .0066943800;  //e'-2  =  f*(2-f)  where  f  =  1/298.257223563  for  WGS84 
const  float  a  =  6378.13700;  //radius  Earth  in  km  for  WGS84 

float  R1 ,  R2 ;  //meridional  radius  of  curvature  and  radius  of  curvature  for  prime  vertical  in  km 
const  float  position. accuracy  =  5;  //  5m  accuracy  for  target  position 
const  float  heading. accuracy  =  10;  //  +/-  10  degress  accuracy 
float  spd;  //Forward  velocity  of  the  robot  in  meters  per  second 

float  turnrate;  //Turn  rate  of  the  robot  in  radians  per  second,  postive  is  clockwise 
double  myLAT ; 
double  myLONG ; 
int  WPcount ; 


*  Robot  Utility  Functions  * 

/*  normal i ze2PI ( double  angle) 

*  Normalizes  an  angle  so  that  it  is  between  0  and  2PI 

*  Oparam  theta:  angle  (radians)  to  be  normalized 

*  ©return  an  equivalent  angle  on  a  range  of  [0,  2PI) 
*/ 

double  normal ize2P I ( double  angle)  { 

while  (angle  >=  (M.PI  *  2.0))  angle  -=  (M.PI  *  2.0); 
while  (angle  <  0.0)  angle  +=  (M.PI  *  2.0) ; 


the 

f  w/  in 


31 


return  angle ; 


/*  normalizePI (double  angle) 

*  Normalizes  an  angle  so  that  it  is  between  -PI  and  PI 

*  Oparam  theta:  angle  (radians)  to  be  normalized 

*  ©return  an  equivalent  angle  on  a  range  of  (-PI,  PI] 

*/ 

double  normal izeP I ( double  angle)  { 

while  (angle  >  M_PI)  angle  -=  (M_PI  *  2.0) ; 
while  (angle  <=  -M_PI)  angle  +=  (M_PI  *  2.0) ; 
return  angle ; 

} 

/*  di St ance ( double  xl ,  double  yl ,  double  x2 ,  double  y2) 

*  Computes  the  linear  distance  between  two  Cartesian  points. 

*  ©param  xl :  Cartesian  X  coordinate  of  the  first  point 

*  ©param  yl :  Cartesian  Y  coordinate  of  the  first  point 

*  ©param  x2 :  Cartesian  X  coordinate  of  the  second  point 

*  ©param  y2 :  Cartesian  Y  coordinate  of  the  second  point 

*  ©return  the  linear  distance  between  (xl,  yl)  and  (x2 ,  y2) 

*/ 

double  di St anc e ( double  xl ,  double  yl ,  double  x2 ,  double  y2)  { 
return  sqrt((x2  -  xl)  *  (x2  -  xl)  +  (y2  -  yl)  *  (y2  -  yl)); 

} 

/*  headingTo (double  xl ,  double  yl ,  double  x2 ,  double  y2) 

*  Computes  a  heading  to  go  from  one  point  in  Cartesian  space  to  another 

*  ©param  pi:  Cartesian  X  coordinate  of  the  first  point 

*  ©param  yl :  Cartesian  Y  coordinate  of  the  first  point 

*  ©param  x2 :  Cartesian  X  coordinate  of  the  second  point 

*  ©param  y2 :  Cartesian  Y  coordinate  of  the  second  point 

*  ©return  the  bearing  (heading)  from  (xl ,  yl)  to  (x2,  y2) 

*/ 

double  headingTo ( double  xl ,  double  yl ,  double  x2 ,  double  y2)  { 
return  atan2((y2  -  yl),  (x2  -  xl)); 

} 


///////////////////////////////////////////////////////////////////// 

//GPS  declarations 

///////////////////////////////////////////////////////////////////// 

#use  ” gps .lib” 

#define  EINBUFSIZE  127 
#define  EOUTBUFSIZE  127 
#define  MAX.GPS.SENTENCE  200 
#define  READDELAY.GPS  5 
#define  GPS.TIMEQUT  30 

//  tests  the  GPS  sentence  for  horz .  error 
int  gps_horz_error ( char  *sentence); 

GPSPosition  current_pos; 
int  gps_error_count ; 

//  initialization  sentences  for  GPS 

const  char  GPS_9600_Bd  []  =  ” $PGRMC ,,,,,,,,,,  5 ,,,, \r\n " ;  //sets  baud  rate  to  9600  ,  needs  unit  reset  before  it  take 
effect 

const  char  GPS.Reset  []  =  ”  $PGRMI  ,,,,,,,  R\r\n'' ;  //unit  reset  sentence 

const  char  GPS_Enable_Avg  []  =  ”  SPGRMCl  ,,,  2 ,,,,,,,,,, \r\n " ;  //enable  auto  position  averaging  when  stopped 

const  char  GPS_Disable_All  []  =  ” $PGRM0  ,  ,  2\ r \n " ;  //turns  off  all  output  sentences 
const  char  GPS_GGA_Enable  []  =  ”  $PGRM0 , GPGGA  ,  l\r\n  ”  ;  //enables  the  GGA  sentence 

/* 

*  Waypoint 
*/ 

typedef  struct  {  float  lat ;  float  Ion;  char  action;  }•  WP ; 
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WP  waypoints  [13]  ;  //  10  user  waypoints  +  3  for  collision  aviodance 

WP  desired_WP ; 

WP  current_WP; 

int  current_wp_count ;  //  a  counter  to  keep  track  of  waypoints 

///////////////////////////////////////////////////////////////////// 

// Functions 

///////////////////////////////////////////////////////////////////// 

///////////////////////////////////////////////////////////////////// 

//Basic  Patterns 

///////////////////////////////////////////////////////////////////// 

void  runCircle (File  *file,  int  baseSpeed) 

■[ 

long  startPosition ; 
int  counter ; 

startPosition  =  StepGetPos  (2)  ; 
counter  =  10 ; 
to  =  MS. TIMER; 

//module  address,  goal  position,  maximum  velocity,  maximum  acceleration,  wait.f or.reply 


while ( counter  <baseSpeed) 

{ 

ServoSetPwm ( 1 , counter  ,  1); 

ServoSe tPwm ( 2 , counter  *(1/2),  1); 

delay  (250)  ; 

counter  =  counter  +  10; 
logDat a  (file)  ; 

} 

ServoSetPwm  (  1  ,  baseSpeed,  1); 

//sit  =  ServoSetVel  (1  ,  10000 ,  10,  0);  //  motor  #  1 

delay (WAIT) ; 

//module  address,  goal  position,  maximum  velocity,  maximum  acceleration,  wait.f or.reply 
ServoSetPwm  (2  ,  bas eSpeed *  (  1 /2)  ,  1)  ; 

//sit  =  ServoSetVel (2 ,  30000,  10,  0);  //  motor  #  2 

delay (WAIT) ; 

ServoSt art Mo ve  (255 ,0)  ; 

whi le  (  1 ) 

timer  =  MS. TIMER; 

logDat a(file)  ; 
delay  (WAIT) ; 

averagePos i t i on  =  ( lef tPo s i t i on  +  r ight Pos i t i on ) /2 ; 
rightPosition  =  StepGetPos  (2)  ; 

if(  rightPosition  >  15000+ st artPo s i t i on ) 

{ 


St  op  (  )  ; 

break ; 


} 

} 

while ( counter  >0) 

{ 

ServoSetPwm ( 1 , counter  ,  1); 

ServoSetPwm (2 , counter  ,  1); 
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delay  (250)  ; 

counter  =  counter  -  10; 
logDat a  (file)  ; 

} 


void  runLine(File  *file,  int  baseSpeed) 

long  St art Pos i t i on ; 
int  counter ; 

St art Pos it  ion  =  StepGetPos  (2)  ; 
counter  =  10 ; 
to  =  MS. TIMER; 

//module  address,  goal  position,  maximum  velocity,  maximum  acceleration,  wait.f or.reply 
sit  =  ServoSe tVe 1  ( 1  , 70000 ,  10,  0);  //  motor  #  1 

/*  whi le ( counter  < baseSpeed) 

{ 

ServoSetPwm ( 1 , counter  ,  1); 

ServoSetPwm (2 , counter  ,  1); 

delay  (250)  ; 

counter  =  counter  +  10; 
logDat a  (file)  ; 

} 

ServoSetPwm ( 1 ,  baseSpeed,  1);  */ 
delay (WAIT)  ; 

//module  address,  goal  position,  maximum  velocity,  maximum  acceleration,  wait.f or.reply 
sit  =  ServoSetVel (2 ,  70000,  10,  0);  //  motor  #  2 

//ServoSetPwm (2 ,  baseSpeed,  1); 
delay (WAIT)  ; 

//ServoStartMove (255,0)  ; 
whi le  (  1 ) 

timer  =  MS. TIMER; 

logDat a(file)  ; 
delay  (WAIT) ; 

averagePosition  =  ( lef tPo s i t i on  +  rightPosition) /2 ; 
rightPosition  =  StepGetPos  (2)  ; 

sit  =  ServoSetVel (2 ,  70000,  10,  0);  //  motor  #  2 

sit  =  ServoSetVel  (1 ,70000 ,  10,  0);  //  motor  #  1 

if(  rightPosition  >  30000+ st art Po si t i on ) 

{ 


St  op  (  )  ; 

break ; 


} 

} 

/  + 

while ( counter  >0) 

{ 

ServoSetPwm ( 1 , counter  ,  1); 

ServoSetPwm (2 , counter  ,  1); 

delay  (250)  ; 

counter  =  counter  -  10; 
logDat a  (file)  ; 

}  */ 


} 


//*+robotInit=t 
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//** 

//♦♦Description:  Initializes  the  key  components  of  the  robot, 
/ /*♦ inc luding  the  board,  motors,  and  any  variables  that  need 
/ /♦♦ set t ing  . 

//♦♦ 

//* *  par amet er  descriptions: 

//♦♦no  parameters  taken. 

void  robot  Ini t ( VO id ) 


brdinit () ; 
serCrdFlushO  ; 

ch  =  Nmcinit  ()  ; 
setServoParameters  ()  ; 

#if  DEBUG.PRINT 

pr  intf  ("  Modules  found  on  network:  XdXnXn'',  ch); 
#endif 


distanceTilNext  =  0; 

//  initialize  gps  and  compass  variables 
gps_error_count  =  0; 
magVar  =  14 ; 

#if  EXT.SENSORS 

//gps  initialization 
serEopen  (  1 9200 )  ; 
serEwrFlush  () ; 

//send  configuration  sentences  to  GARMIN  GPS 
serEputs (GPS_9600_Bd)  ; 

serEput s ( GPS_Re set ) ; //  need  reset  for  baud  rate  change  to  take  effect 
serEputs (GPS_Enable_Avg) ; 
serEputs (GPS_Disable_All) ; 
serEputs ( GPS_GG A.Enable ) ; 

//Compass  int ializat ion 

serFopen  (  1 9200 )  ;//19200  baud  default  for  Honeywell 
serFwrFlushO  ; 
serFputs(init_str) ; 

#endif 

> 

void  setServoParameters () 

■[ 

//module  address,  position  gain,  derivative  gain,  integral  gain,  integration  limit,  output  limit, 

//current  limit,  error  limit,  servo  rate  divisor,  deadband  compensation,  wait_f or.reply  status  ( 1 - individual ,  0- 
group ) 

successFlag  =  ServoSetGain  (255  ,  100,  10,  2,  1400  ,  255,  0,  4000  ,  1  ,  1  ,  0);  //  0  for  group  addr 

delay (WAIT) ; 

#if  DEBUG.PRINT 

pr int f (" ServoSetGain  success?:  %d\n\n",  successFlag); 

#endif 


//module  address,  control,  wait.f or.reply  (1  -  individual  ,  0-group) 

successFlag  =  ServoSetlOMode  (255  ,  12,  0);  //  for  4  motors  group  addresses 

delay (WAIT) ; 

#if  DEBUG.PRINT 

pr  int  f  (  "  ServoSet  I  OMode  success?:  XdXnXn'',  successFlag); 

#endif 


//This  is  necessary,  initializes  mode. 
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ServoStopMotor(255 ,11  ,0)  ; 


//  group  address  for  all  4  motors; 


successFlag  = 
delay (WAIT) ; 

#if  DEBUG. PRINT 
pr  int  f  (  "  ServoSt  opMo  t  or  success?:  yod\n\n'',  successFlag); 
#endif 


#if  DEBUG.PRINT 

print f ("Initializing  servos\n\n") ; 
#endif 


> 


/**BASIC  MOTOR  COMMANDS 


/ 


void  setLef tSpeed ( long  velocity,  long  acceleration) 

■[ 

//module  address,  goal  position,  maximum  velocity,  maximum  acceleration,  wait.f or.reply 
sit  =  ServoSetVel (1 ,  velocity,  acceleration,  1);  //  motor  #  1 

delay (WAIT) ; 

//slt2  =  ServoSetVel (3 ,  velocity,  acceleration,  1);  //  motor  #  3 

delay (WAIT) ; 


sit  =  0 ; 
//slt2  =  0; 


void  setRightSpeed ( long  velocity,  long  acceleration) 

■[ 

//module  address,  goal  position,  maximum  velocity,  maximum  acceleration,  wait.f or.reply 
sit  =  ServoSetVel (2 ,  velocity,  acceleration,  1);  //  motor  #  2 

delay (WAIT) ; 

//slt2  =  ServoSetVel (4 ,  velocity,  acceleration,  1);  //  motor  4 

//delay (WAIT) ; 


sit  =  0 ; 

//slt2  =  0; 

> 

/**END  BASIC  MOTOR  COMMANDS**************/ 

/**ADVANCED  MOTOR  COMMANDS**************/ 
void  stop  () 

■[ 

// setLef tSpeed  (0 ,  Acceleration); 

// setRightSpeed (0,  Acceleration)  ; 

//delay  (2000)  ; 

ServoStopMotor  (255 ,  11,  0);// 

> 

void  goStraight ( long  velocity,  long  acceleration,  int  direction  )//l  for  forward,  -1  for  reverse 

■[ 

//distance  you  want  to  go,  velocity,  acceleration 
setLef tSpe ed ( dir ect i on *( vel oc ity  -78)  ,  acceleration);  //  was  -75 

setRightSpeed ( dir e ct ion* veloc it y ,  acceleration) ; 

ServoStartMove (255,0)  ; 

} 


/**END  ADVANCED  MOTOR  COMMANDS**********/ 


/**DATA  STORAGE  COMMANDS **************/ 
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void  logDataCFile  *f) 

{ 

char  bufferl  [7]  ; 

char  buff  1  [7]  ,buff2  [5]  ,  buff3[5],  buff 4  [5]  ,  buff5[5],  buff6[8],  buff7[15],  buff8[15]; 


//  TIMER 


timer  =  MS_TIMER-tO; 
spr int f ( buff  1  , " %d  ",  timer) ; 
fwrite(f, buff  1  , sizeof (timer) ) ; 
if(  DATA. PRINT) 

printf("%s  ", buffi); 


f write  (f  ,  "  "  ,  1)  ; 


// serCrdFlush  ()  ; 


//  LEFT  POSITION 


leftPosition  =  St epGetPo s  (  1 )  ; 
spr int f ( buff 2 ,"%d  ", leftPosition); 
fwrite(f, buff 2 , sizeof (leftPosition))  ; 
if(  DATA.PRINT) 
printfC  ‘/(S  ",buff2); 

f write  (f  ,  "  "  ,  1)  ; 

//  LEFT  CURRENT 


MIAD  =  StepGetAD  (1)  ; 
sprintf (buf f 3 , "%d  ",M1AD); 
fwrite(f, buff 3 ,sizeof(MlAD)); 
if(  DATA.PRINT) 
printfC  ‘/eS  ",buff3); 

f write (f  ,  "  "  ,  1)  ; 


//  RIGHT  POSITION 


rightPosition  =  StepGetPos  (2)  ; 
sprintf (buff 4 , "%d  ", rightPosition) ; 
fwrite(f, buff 4 , sizeof (rightPosition) ) ; 
if(  DATA.PRINT) 
printfC  ‘/eS  ",buff4); 

f write  (f  ,  "  "  ,  1)  ; 

//  RIGHT  CURRENT 


M2AD  =  StepGetAD  (2)  ; 
sprintf (buff 5 , "%d  ",M2AD); 
fwrite(f, buffs ,sizeof(M2AD)); 
if(  DATA.PRINT) 
printfC  ‘/eS  ",buff5); 

//  HEADING 


sprintf  (buff  6  ,  "  ’/,3.0f  ".heading); 
fwrite(f, buff 6 , sizeof (heading ) ) ; 
if(  DATA.PRINT) 
printfC  %s",buff6); 


//  LAT 


sprintf  (buff  7  ,  "  ‘/,f  ",myLAT); 
fwrite(f, buff 7  ,10)  ;//sizeof (my LAT ) ) ; 
if(  DATA.PRINT) 
printfC  ‘/eS  ",buff7); 
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//  LONG 


sprintf (buf f 8 , "  %f  ".myLONG); 
fwrite(f, buffs ,10) ;//sizeof (myLONG ) 
if(  DATA. PRINT) 

printfC  ‘/,s  \n "  ,  buf  f  8  ,  s ize of  (myLONG  )  )  ; 


f write (f , " \n"  ,  1) ; 

} 

/**END  DATA  STORAGE  COMMANDS**** 


ifif/ 


1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

//4.  compass  (RS232) 

///////////////////////////////////////////////////////////////////// 


///////////////////////////////////////////////////////////////////// 

//  Function:  msDelay 

//  Input : desired  time  delay  in  ms 

//  Output :  Nil 

//  Description : To  delay  the  processor  with  the  input  time  in  ms,  primarily 
//  for  serial  and  I2C  communications 
///////////////////////////////////////////////////////////////////// 
void  msDelaydong  sd)  { 

unsigned  long  tl;  tl  =  MS.TIMER; 

for  (tl  =  MS.TIMER;  MS.TIMER  <  (sd  +  tl);)  ; 

} 

///////////////////////////////////////////////////////////////////// 

//  Function:  GetCompassString 
//  Input:  char  ptr  to  CompassString 
//  Output :  Nil 

//  Description:  Function  fills  buffer  with  serial  data  from  serialB  data 
//  from  the  Honeywell  compass.  Parses  the  buffer  and  returns  the  compass 
//  sentence  beStween  $  and  *. 

///////////////////////////////////////////////////////////////////// 

void  Ge tCompas sSt ring ( char  * Re t Compas sSt r )  { 

char  error. buf  [100]  ; 

//waitfor  (  DelayMs ( c ompass .del ay ) ) ; 
rasDelay(COMPASS. DELAY)  ; 

serFrdFlush  ()  ; 
string. pos  =  0; 

#if  EXT. SENSORS 

input. char  =  serFgetcO; 

//find  begining  of  sentence 
while  (input. char  !=  ’$’)  { 
input. char  =  serFgetcO; 

#if  DEBUG. PRINT 

pr  int  f  (  "  %  c ,  input. char); 

#endif 

msDelay (READDELAY) ; 

} 

#if  DEBUG. PRINT 
pr int f ( " \n " ) ; 

#endif 

//read  the  sentence 

while  (input. char  !=  ’*’)  { 
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compass.sentence [string.pos]  =  input.char; 
string_pos++; 

if  (string. pos  ==  MAX.SENTENCE) 

string.pos  =  0;  //reset  string  if  too  large 

input.char  =  serFgetcO; 

#if  DEBUG. PRINT 

pr  int  f  (  "  %  c ,  input.char); 

#endif 

msDelay(READDELAY) ; 

} 

compas s .s ent ence [ St r ing.pos ]  =  0;  //add  null 

if  (( compass. error  =  compass ( c ompass . sent ence ) )  !=  0)  { 

sprintf ( error. buf ,  "$Compass  Error:  XdXn",  c ompass .error ) ; 

//send  error  to  the  COMM  module 
//strcat (ErrString ,  error. buf); 

#if  DEBUG. PRINT 

printf  (  ”  $Compass  Error:  ’/,d\n  ‘/,s\n'',  compass. error  , 
compass . sent ence )  ; 

#endif 

}  else  { 

#if  DEBUG. PRINT 

printf  C  Current  heading:  %03.0f\n'',  heading); 

#endif 

// Compass .updat e  =  1;  //gobal  variable 

} 

// sprint f ( Ret Compas sStr ,  XOS.OfXn”,  heading); 

#endif 

#if  ! EXT. SENSORS 

St  r  cpy  (  compass  .sent  ence  ,  "  $PTNTHPR  ,  301  ,  N  '' )  ; 

#endif 

} 

///////////////////////////////////////////////////////////////////// 

//  Function:  compass 
//  Input:  char  sentence 
//  Output  :  int 

//  Description:  Sets  the  floating  point  heading  global  variable 
//  after  parsing  the  sentence  passed  to  the  function. 
///////////////////////////////////////////////////////////////////// 
int  compass(char  sentence [MAX.SENTENCE] )  { 
auto  int  i ; 

char  *err,  *hdg ,  *type; 
char  error ; 

if  ( strlen ( sentence )  <  4) 
return  -1 ; 

if  ( strncmp ( sent ence ,  "SPTNTHPR",  8)  ==  0)  { 

//parse  hpr  sentence 
type  =  strtok ( sentence  ,  ","); 

hdg  =  strtok( NULL  ,  ",''); 

err  =  strtok( NULL  ,  ",''); 

if  (hdg  ==  NULL) 
return  -2 ; 

//pull  out  data  -  update  global  variable  and  add  magVar  to  get  trui 
heading  =  atof(hdg)  +  magVar; 
if (heading  >  =  360.0) 

heading  =  heading  -  360.0; 

error  =  ( int )  err  ; 

if  ( strncmp (&error ,  "N",  1)  ==  0) 

return  -2 ; 

)■  else 

return  -1 ; 


heading 
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return  0; 


} 


/* 

*  GPS 
*/ 

///////////////////////////////////////////////////////////////////// 

//  Function:  gps 
//  Input  :  None 
//  Output :  None 

//  Description:  This  function  will  get  GPS  GGA  string  through  serial  comms  on 
//  RS232  port  C,  and  parse  the  string  onto  global  variable  for  current  gps 
//  position.  Added  a  check  for  horizontal  error  for  gps_error_count . 

///////////////////////////////////////////////////////////////////// 

void  gpsO  -C 

char  sentence [MAX.GPS.SENTENCE]  ; 

int  input.char; 

int  string_pos; 

int  gps_error ; 

unsigned  long  t; 

t  =  MS.TIMER; 

serErdFlush () ; 
string.pos  =  0; 

#if  EXT.SENSORS 

input_char  =  serEgetcO; 

//printf  ( '' c  :  Xc  \n",  input_char); 

while  (input_char  !=  ’$’)  //find  begining  of  sentence 

< 


input.char  =  serEgetcO; 
msDelay(READDELAY.GPS) ; 

//printfC'c:  %d  \n'',  input_char); 

} 

while  ((input_char  !=  ’\r’)  kk  (input_char  !  =  ’\nO) 

sentence  [string.pos]  =  input.char; 
string.pos  ++ ; 

if (string.pos  ==  MAX.GPS.SENTENCE) 

string.pos  =  0;  //reset  string  if  too  large 
input.char  =  serEgetcO; 
msDelay(READDELAY.GPS) ; 

} 

#endif 

#if  ! EXT.SENSORS 

//  pass  a  dummy  GPS  packet  if  there  are  no  external  sensors  set  up 
strcpy  (  sentence  ,  "  $GPGGA  , 123456, 3635. 7058, N, 12152. 4939, W, 2, 5''); 

#endif 

#if  DEBUG. PRINT 

pr  int  f  (  "  Xs \n ,  sentence); 

#endif 

//  SendToCommModule (&GpsChan ,  sentence); 

gps. error  =  gps. get .pos it i on (& current .pos ,  sentence);  //parse  lat  and  long  position  with  gps  lib  function 

if  (gps. error  ==  0)  { 

//  get  horizontal  nav  error 

if  ( ! gps .horz.err or ( sent ence ) )  //if  no  error,  then  GPS  accuracy  is  acceptable 
■[ 

gps. error. count  =  0; 

//dr. mode. flag  =  0; 

y  else  //sentence  is  OK,  but  horizontal  dilution  of  precision  exceeds  limit 
//or  gps  reported  a  sentence  with  no  position  fix 
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gps.err or_c  ount  ++ ; 

// if ( gps_error_ count  >  3) 

// dr_mode _f lag  =  1;  //start  dead  reckoning  nav . 


> 

}•  else  //  gps  parsing  error  or  invalid  sentence 
■[ 

gps.err or _  count  ++ ; 

// if ( gps .error _ count  >  3) 

//  dr_mode_flag  =  1;  //start  dead  reckoning  nav. 

} 

sentence [string.pos]  =  0; 

} 

///////////////////////////////////////////////////////////////////// 

//  Function:  gps.horz.error 
//  Input:  char  *sentence 

//  Output:  int  result  0  =  no  error,  1  =  error  in  horizontal  positioning 
//  Description:  This  function  parses  the  gps  GGA  sentence  to  extract  the 
//  horizontal  dillution  of  precision  and  quality  of  the  gps  fix.  dop 
//  greater  than  roughly  6.0  will  result  in  poor  fix  accuracy. 
///////////////////////////////////////////////////////////////////// 
int  gps_horz_error ( char  *sentence)  { 
auto  int  i ; 
char  *  charPtr ; 
char  *  dummy ; 

char  s  [6] ;  //temp  string  for  the  dop  chars 
float  dop;  //dilution  of  precision 

strcpy(s,  " ;  //set  to  NULL 

if  ( strncmp ( sent ence ,  "$GPGGA",  6)  ==  0)  { 

//parse  GGA  sentence 
for  (i  =  0;  i  <  9;  i++)  { 

sentence  =  strchr ( sentence  ,  ’  ,  ; 

if  (sentence  ==  NULL) 

return  1;  //return  true,  horz .  error  exists 

sentence++;  //first  character  in  field 
//pull  out  data 
if  (i  ==  5)  //link  quality 
{ 

if  (*sentence  ==  ’O’)  //indicates  no  fix 

{ 

#if  DEBUG. PRINT 

printf ("No  fix\n"); 

#endif 

return  1;  //return  true,  horz.  error  exists 

} 

} 


//  DGPS  fix  can  still  have 
//  GPS  can  have  acceptable 
//  Test  satellite  geometry 
//  before  accepting  fix. 


excessive  horizontal  errors,  while 

fix  accuracy  if  good  satellite  geometry. 

(Horizontal  Dilution  of  Precision,  HDOP) 


//  get  the  HDOP  measure  of  the  geometry.  If  it’s  OK, 

//  consider  the  fix  GOOD.  If  HDOP  too  big,  return  1  =  error. 

//  Based  on  experimental  observation  in  Quad... 

//  Typical  HDOP  for  excellent,  6  satellites  DGPS  fix  is  1.1  to  1.7 
//  Adequate  GPS  fixes,  HDOP  varies  from  2.9  to  up  to  5.5 
//  4  satellite  GPS  fixes  with  HDOP  =  8  or  9  are  about  20  to  30  m  off 
if  (i  ==  7)  { 

//get  the  chars  from  sentence  to  the  next  ’,’ 

charPtr  =  st rchr ( sent ence ,  ’,’);  //find  the  location  of  , 

strncat(s,  sentence,  (charPtr  -  sentence)); 

//convert  the  chars  to  float 
dop  =  strtod(s,  &dummy); 
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#if  DEBUG. PRINT 

printfC'gps  dop  XfXn",  dop); 

#endif 

//test  dop 
if  (dop  >  5.6) 

return  1;  //horizontal  error  too  big 

} 

y  / / end  for  loop 
)•  //end  if  good  GGA  sentence 

else  //error  not  GGA  sentence 

return  1 ; 

} 

return  0;  //everything  else  is  OK,  so  no  error 


///////////////////////////////////////////////////////////////////// 

//6.  Navigation 

///////////////////////////////////////////////////////////////////// 

////////////////////////////////////////////////////////////////////////////// 

//  Function:  navigation 
//  Input  :  Nil 
//  Output  :  Nil 

//  Description:  Uses  a  potential  field  alogrithm  which  simulates  waypoints  as 
//  linear  points  of  attractions  and  obstacles  as  l/r~2  points  of  repulsion. 

//  This  apporach  encompasses  both  waypoint  driving  and  object  avoidance  simultaneously. 

// 

////////////////////////////////////////////////////////////////////////////// 

void  nav igat i on ( VO  id )  { 

float  err.angle;  //Difference  between  current  heading  and  intended  heading 

float  angle.to.goal ;  //Bearing  to  waypoint  from  gps  location,  independent  of  robot  heading 

float  turn.gain;  //Gain  for  proportional  turn  rate 
float  speed.gain;  //Gain  for  propotional  spd  rate 
float  min.speed;  //Minimum  spd  for  search  motion 
float  max.speed;  //Maximum  spd  for  search  motion 

//float  laserdat a [ St ate . laserNumRe turns ]  [1]  ;  //Processed  laser  bearing  and  range  data  matrix 
auto  int  i;  //index  for  processing  laser  information  into  laserdata 

float  repgain;  //Gain  factor  for  rep  force 
float  repmag ;  //Magnitude  for  rep  force 

float  repdir;  //Polar  direction  for  direction  of  rep  force 
float  force.rep.x;  //Computed  x  force  component  of  rep  force 
float  force.rep.y;  //Computed  y  force  component  of  rep  force 
float  sumf orce.rep.x ;  //Summation  of  x  force  component  of  rep  force 

float  sumf orce.rep.y ;  //Summation  of  y  force  component  of  rep  force 

float  attgain;  //Gain  factor  for  att  force 
float  attmag;  //Magnitude  for  att  force 

float  attdir;  //Polar  direction  for  direction  of  att  force 
float  force.att.x;  //Computed  x  force  component  of  att  force 
float  force.att.y;  //Computed  y  force  component  of  att  force 
float  sumf or ce .at t _x ;  //Summation  of  x  force  component  of  att  force 

float  sumf or ce .at t .y ;  //Summation  of  y  force  component  of  att  force 

float  totalf orce.x ;  //Addition  of  att  and  rep  force  x  component 
float  totalf orce.y ;  //Addition  of  att  and  rep  force  y  component 

double  current. lat,  current. Ion; 

double  wypt.lat,  wypt.lon;  //lat  and  long  for  waypoint 
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//find  Origin  of  the  flat  Earth.  Let  Origin  correspond  to  robot’s  current  position 

// cur rent _lat  =current_WP . lat ; 

// cur rent _lon  =current_WP . Ion ; 

curr ent _ lat  =  ( current _pos . lat _minut e s  /  60)  ; // current _pos . lat_degrees  + 

if  ( current _po s . lat .direct i on  ==  ’S’) 

current.lat  =  0  -  current.lat;  //negative  values  in  Southern  hemisphere 

current.lon  =  ( current.pos . lon.minutes  /  60);  //+current_pos . lon.degrees 


if  ( current _po s . lon.direct i on  ==  ’W’) 

current.lon  =  0  -  current.lon;  //negative  values  for  West  hemisphere 


#if  NAV.PRINT 

pr  int  f  ("  CURRENT  :  ‘/,f  ’/,f\n",  current.lat  ,  current.lon)  ; 

#endif 

//find  the  lat,  Ion  of  the  point  to  navigate  to  from  the  Origin 
wypt.lat  =  desired.WP . lat ; 
wypt.lon  =  desired.WP . Ion ; 

#if  NAV.PRINT 

pr  int  f  (  "  DES IRED  :  ‘/,f  ’/,f\n",  uypt.lat  ,  wypt.lon)  ; 

#endif 

myLAT=  current. lat; 
myLQNG=  current. Ion; 

error. distance  =  di stance ( curr ent .Ion ,  current. lat,  wypt.lon, 

wypt.lat);  //Error  distance  between  goal  and  starting  position 


if (  WPcount  ==  2  &&  error. distance <  DISTANCE. ERROR) 
WPcount  =  3 ; 

if (  WPcount  ==  1  &&  error. distance <  DISTANCE. ERROR) 
WPcount  =  2 ; 


desired.heading  = ( 1 80 . 0/ M.PI ) *  normal ize 2PI ( M. PI /2  -  headingTo ( current. Ion ,  current. lat 
wypt.lat));  //Bearing  to  waypoint  from  gps  location,  independent  of  robot  heading 
#if  NAV.PRINT 

printfC'D  HEADING:  ’/,f\n",  desired.heading); 

#endif 


} 


///////////////////////////////////////////////////////////////////// 
//  Function:  drive. heading 
//  Programmer:  Jess  Fitzgerald 

//  Description:  navigates  to  and  maintains  requested  course 

// 

///////////////////////////////////////////////////////////////////// 

void  drive.heading ( float  requested. heading) 

float  modification; 
float  Lvel,Rvel; 

desired.heading  =  requested.heading ; 

#if  HEADING. PRINT 

pr int f (" Heading  %03.0f\n\n",  heading); 

#endif 


,  wypt.lon , 
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modification  =  PID_Controller  ()  ; 

#if  PID.PRINT 

pr int f  (  "  Mod  :  Xf  ,  modif  ication)  ; 

#  endi f 

Rvel =BASE_SPEED -modif ication; 

Lvel =BASE_SPEED+modif ication; 

//  #if  PID.PRINT 

//  printfC  LiXB.Of  RiXS.Of  \n\n  ”,  Lvel  ,  Rvel  )  ; 

//  #endif 

ServoSetPwm ( 1 , Rvel ,  1); 

ServoSetPwm (2 , Lvel  ,  1); 

} 

///////////////////////////////////////////////////////////////////// 

//  Function:  PID  Controller 
//  Programmer:  Jess  Fitzgerald 

//  Description:  Uses  error,  error  differential  and  error  sum  to  compute 
//  hov?  much  effor  should  be  put  into  getting  back  on  track 
///////////////////////////////////////////////////////////////////// 


float  PID.Controller (void) 

float  error,  d.heading;  //  d.heading  is  the  difference  between  the  current 

//  heading  and  the  last  heading 

float  returnValue ; 

//  Proportional  Control 
#if  PID.PRINT 

printf  ("Xd  A:‘/.03.0f  D  :’/, 03 . 0  f \n  ”,  MS. TIMER  ,  heading  ,  des  ired. heading  ); 

#endif 

error  =  desired. heading -heading ; 

if ( error <  - 1 80)  //gives  relative  bearing  from  -180  to  180 

error  =  error  +  360; 
if ( error  >180) 
error  =  error  -  360; 

//#if  PID.PRINT 

//  printfC  E  : '/c03 . 0  f  ”  ,  err  or  )  ; 

/ /#  endi f 

if(error<2.0  &&  error  >  -2.0) 
error. sum=0 ; 

else 

error. s urn += error  ;  /  /I ; 
if  (error. sum  >180  I  I  err or .sum <- 1 80) 
error. sum  =  0 ; 

//  Derivative  Control 

d.heading  =  heading  -  previous. heading ;  //heading  derivative 

previous. heading  =  heading; 
previous. error  =  error; 

returnValue  =  (  k.p*error  +  ( 1/k.i ) *  err or .sum  -  (k.d) *d. heading) ; 

//  #if  PID.PRINT 

//  printfC  R:  %1.3f  ”,  returnValue); 

//#endif 


return  returnValue; 

} 


main  () 

File  filel; 
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File  file2; 

char  CompassString  [100]  ;  //for  comms  to  control 
char  dir_string;  //for  gps  print 
char  val  [20]  ; 

robotinit () ; 
f s_get_ram_lx () ; 
fs_init (0,0) ; 

desired_WP . lat  =  .603010; 

desired.WP . Ion  =  -.873840;  //longitudes  from  laptop  are  inverse  polarity,  so  multiply  by  -1 
desired.WP . action  =  ’a’; 

WPcount  =  1 ; 

// current.WP . lat  =36.5904889; 

//current. WP . Ion  =-121.8716511; 

// current _WP . act i on  =  ’a’; 

error_sum=0 ; 


if  (! f create (&filel ,  TESTFILE)  &&  f open. wr  (& file  1  , TESTFILE ) ) 

printf ("\n\n  error  opening  TESTFILE  XfXn",  errno); 
return  - 1 ; 

} 

if (! f create (&file2 ,  TESTFILE2)  kk  f open.wr (&f ile2 , TESTFILE2 ) ) 

printf  ("\n\n  error  opening  TESTFILE2  ‘/cf\n'',  errno); 
return  - 1 ; 

} 

fclose(&filel); 

fclose(&file2); 

f delete  (  1 )  ; 
f delete  (2)  ; 

if (! f create (&filel  ,  TESTFILE)  kk  f open.wr (&f ilel ,  TESTFILE )  ) 

■{ 

printf ("\n\n  error  opening  TESTFILE  XdXn",  errno); 
return  - 1 ; 

} 

if (! f create (&file2 ,  TESTFILE2)  kk  f open.wr (&f ile2 , TESTFILE2 ) ) 

printf  ("\n\n  error  opening  TESTFILE2  %d\n'',  errno); 
return  - 1 ; 

} 


// runLine  (& f ile 1  ,  240); 

// delay  (500 )  ; 

// runCi r cle (& f ile 1 ,  200); 

//stop  ()  ; 

/ / de  s ired.heading  =  l 90 ; 

/* ServoSetPwm  (  1 , 250  ,  1); 

ServoSet Pwm  (  2 , 50  ,  1); 

delay  (10000)  ;  */ 

to  =  MS. TIMER; 
desired. heading =50; 

while  (!  kbhit  ())  ■{  //  ( MS. TIMER  <t0 +6000)  •( 
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costate  {  //compass 

Get CompassSt ring (CompassString) ; 
compass_error  =  compass ( c ompas s.sentenc e ) ; 

// SendToCommModule (& Compass Chan ,  CompassString) ; 
// SendCompas  sToCont  r 0 ISt  at  ion  (  )  ; 

//tcp.tick (NULL) ; 

//  wait f or ( DelayMs  ( 50) )  ; 
y  / /  end  costate  */ 


costate  {  //gps 
gps  ()  ; 

#if  DEBUG.PRINT 

dir_string  =  current_pos . lat_direction ; 
printf  (  "  Lat  itude  :  %d  %g  ’/,c\n",  current_pos  .  lat.degrees  , 
current_pos . lat .minutes ,  dir.string) ; 
dir.string  =  current _pos . 1 on.dire ct i on ; 

printf (" Longitude :  Xd  %g  %c\n",  current.pos . lon.degrees  , 
current.pos . lon.minutes ,  dir.string) ; 

#endif 

//  SendGpsToCont rolSt at i on  ( )  ; 

//  t cp_t ick ( NULL )  ; 

//  wait f or ( DelayMs (1000 ))  ; 

y  / /  end  costate 

costate  {  //Drive  to  desired  Heading 


if(WPcount  ==  2) 

■[ 

desired.WP . lat  =  .  603254 ;//. 59508418 ; //36 . 59038613 ; 

desired.WP . Ion  =  873187 ;//-. 87496038 ; 

multiply  by  -1 
desired.WP . action  =  ’a’; 

> 


if(WPcount  ==  3) 


desired.WP . lat  =  . 603150 ;//. 59508418 ; //36 . 59038613 ; 

desired.WP . Ion  =  873288 ;//-. 87496038 ; 

multiply  by  -1 
desired.WP . action  =  ’a’; 


//longitudes  from  laptop  are  inverse  polarity,  so 


//longitudes  from  laptop  are  inverse  polarity,  so 


> 


navigation () ; 

dr ive .heading ( des ired. he ading );// Thi s  will  drive  to  the  most 

//recently  calculated  desired 
//heading  stored  in  the  global 
//varialble  desired. heading . 


1 ogDat a (& f i le 1 ) ;  //LOG  data  from  current  run 

//printf  ("Xf \n''  ,  err  or  .di  stance  )  ; 

if  (kbhitO  II  (error. distance <DISTANCE. ERROR  kk  (WPcount  ==  2)))  // kbhi t  ( )  I  I MS.TIMER > 1 0 +  60000) 

{ 


fclose(&filel); 

fclose(&file2); 

stop  ()  ; 
abort  ; 

} 

// t cp.t i ck ( NULL ) ; 
//waitfor( DelayMs (120) ) ; 
y  //  end  costate 
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y  / / end  while 


fclose  (fcfilel)  ; 
fclose (&file2)  ; 
delay  (1000)  ; 
stop  ()  ; 

if (  DEBUG.PRINT  ||  DATA. PRINT) 
printfC  Ending  program  \n"); 

> 
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APPENDIX  B: 

Data  Extraction  Code 

/*  F i t zger ald.Thes i s_20 1 3  .  c 

*  Programmers:  Jessica  L.  Fitzgerald 

*  Programs  Referenced/Used:  Z-world ’ s ’ DUMPFILE . C ’  from  file  system  sample  files 

*  Supervisor:  Prof  Harkins 

* 

*  Program  Description:  Used  to  extract  Data  stored  in  RAM  by 

*  ’ Fitzgerald_Thesis_2013 . c ’  which  formats  the  string  before  storage. 

*/ 

#class  auto 

#define  MY.FORCE.FQRMAT  0 

#define  FS2_USE_PR0GRAM. FLASH  16 

/** 

*  LX_2_USE  -  You  can  use  f s _get _f lash.lx  ( )  (for  the  2nd  flash); 

*  f s_get _r am_lx ( )  (for  RAM,  if  configured  in  BIQS\RABBITBIQS . C) ; 

*  or  f s_get_other_lx ()  (for  program  flash  if  configured  in 

*  \BIQS\RABBITBIQS . C) . 

*/ 

#define  LX_2_USE  f s_get _r am_lx ( ) 

#define  FS.DEBUG 
#define  FS.DEBUG.FLASH 
#memmap  xmem 
#use  ''  f  s2 . 1  ib  '' 

#use  "errno.lib" 

FSLXnum  fs_ext; 

/*  -  */ 

/*  START  FUNCTION  DESCRIPTION  ******************************************** 
mem.dump 

SYNTAX:  void  mem_dump(  const  char  *  real.ptr ,  long  addr ,  long  d.size  ) 

KEYWORDS:  debug 

DESCRIPTION:  Formats  bytes  in  root  memory.  If  addr  !=  -IL,  then  bytes 

fetched  from  real_ptr  but  displayed  as  if  from  addr. 

0123456789ABCDEF 
543210:  xx  xx  xx  xx  xx  xx  xx  xx_xx  xx  xx  xx  xx  xx  xx  xx  1 0 1 23456789 ABCDEF 1 

RETURN  VALUE:  none. 

SEE  ALSO:  printfO 

END  DESCRIPTION  **********************************************************/ 

//  Separator  between  byte  7  and  8. 

#define  CH.BETWIX  ’\\’ 

nodebug  void 

mem_dump(  char  *  real_ptr ,  long  addr,  long  d_size  ) 

static  const  char  hexdigs  []  =  " 0 1 23456789 ABCDEF " ; 

auto  char  row  [16]; 

auto  char  str  [80]  ; 
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auto  char  pstr  [80]  ; 

auto  char  *  bp;  /*  hex  byte  ptr  */ 

auto  char  *  ap ;  /*  ASCII  rendering  pointer  */ 

auto  char  j  ; 

/*  If  not  faking  address ,  set  it  .  .  .  */ 

if(  -IL  ==  addr  )  { 

addr  =  (long)  real.ptr; 

} 

whileC  d.size  >  0  )  { 

/*  The  magic  constants  here  define  where  the  hex  and  ASCII  areas  are.  */ 
sprintf(  str  ,  "%051x:  ",  addr  &  OxFFFFFFFOL  ); 

memset  (  str  +  7,  ’  s izeof ( st r ) - 1 -7  ); 

str[3l]  =  CH.BETWIX; 

str  [57]  =  ’  I  ’ ; 

str  [74]  =  ’  I  ’ ; 

str  [75]  =  ’\0’; 

j  =  ((char)addr)  &  OxOF; 
bp=str+8+j*3; 
ap  =  str  +  58  +  j  ; 

memcpy  (  row  +  j ,  real_ptr  ,  16  -  j  )  ; 

/♦ADVANCE*/ 
real_ptr  +=  (16  -  j); 
addr  +=  (16  -  j )  ; 

/*  Convert  to  printable  ASCII  ,  if  applicable  ,  default  is  ’  .  ’  */ 
for(  ;  d_size  >  0  &&  j  <  16  ;  ++j ,  --d_size  )  { 

*bp++  =  hexdigs [  row[j]  >>  4  ] ; 

*bp++  =  hexdigs  [  row[j]  &  15  ]  ; 

*  ap  =  ’  .  ’ ; 

if (  ’  ’  <=  row[j]  kk  row[j]  <=  )  { 

*ap  =  row[j];  /*  If  printable  ASCII,  then  display  it.  */ 

} 

ap  +  + ; 

bp++;  //  skip  space  between  hex  numbers. 

} 

printf(  "’/,  s\n",  str); 

} 

}  /*  end  mem.dumpO  */ 

/** 

*  Dump  the  file  contents. 

*/ 

void 

do_dump(int  fnum) 

char  contents  [128]  ; 

File  f; 

int  want  ,  got , count  ; 

long  addr ; 

char  buffer  [1]  ,  time  [5]  ,leftE[5]  ,  rightE  [5]  ,leftl  [2]  ,  right  I  [2]  ; 
count  =  1 ; 

if(  0  !=  fopen_rd(  k  f,  fnum  )  )  { 

printf(  "\nError:  file  #’/,d  doesn’t  exist  (errno  yod)\n",  fnum,  errno  ); 
exit (2)  ; 

} 

addr  =  OL ; 

while (fread(&f, buffer  ,  1 )  >0) 
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pr  int  f  (  "  Xs  ''  ,  buffer  )  ; 


//if(count'/,5  ==  0) 

//  printf  (  "  \n  '' )  ; 
count  =  count+1; 

> 

/*uhile(  0  !=  (got=fread(  &  f,  contents,  s izeof ( content s )  ))  )  { 
mem_dump(  contents,  addr ,  got  ); 
addr  +=  got ; 

}  */ 

f close  (  &  f  )  ; 

}  /*  end  do.dumpO  */ 

/  *  * 

*  Create  a  file  with  some  contents.  Lots  of  error  checking  here. 

*/ 

void 

do_create ( int  fnum) 

auto  char  contents  [128]  ; 

auto  File  f  ; 

auto  int  want,  got; 

auto  long  addr; 

/*  fopen_wr()  if  file  exists,  or  fcreateO  if  doesn’t  exist.  */ 
if(  0  !=  fopen_wr(  &  f,  fnum  )  &&  0  !=  fcreateC  &  f,  fnum  )  )  { 

printfC  "\nError:  file  #’/,d  not  writable  (errno  yod)\n",  fnum,  errno  ); 
exit (2)  ; 

} 

sprintf(  contents,  "This  is  a  test  1234  of  FileSystem  Mkll\n"  ); 

if (  0  ==  fwrite(  &  f  ,  contents  ,  st rlen ( cont ent s )  )  I  I 

0  ==  fwriteC  &  f,  contents,  strlen(contents)  )  II 
0  ==  fwriteC  &  f,  contents,  strlen ( contents )  )  )  { 
printfC  "error:  writing  Cerrno  yod)\n",  errno  ); 

} 

f close  C  &  f  )  ; 

}  /*  end  do.createC)  */ 


*  Try  opening  Cfor  reading)  every  file.  Print  those  we  can  do! 

*/ 

void 

s can_f  or _ex i s t ing_f i 1 e s  C ) 

auto  File  f  ; 

auto  int  fnum ; 

auto  int  count ; 

printfC  "Found  these  existing  files:  "  ); 
forC  f num=l ,  count  =  0  ;  fnum  <  128  ;  ++  fnum  )  { 
ifC  0  ==  fopen.rdC  &  f,  fnum  )  )  { 
f close  C  &  f  )  ; 
printf  C  "  #yod  "  ,  fnum  )  ; 

++count ; 

} 

} 

if  C  0  ==  count  )  •( 

printf  C  "  ...  none  ..."  )  ; 

} 

printf  C  "\n"  )  ; 

}  /*  end  scan.f or _exi St ing_f i les C)  */ 
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/ 


void 
main  () 

{ 

char  contents  [  128  ]; 
int  re ; 

int  f num ; 

errno  =  0; 
fs.ext  =  LX_2_USE; 
if  ( ! f  s_ext  )  { 

printfC'The  specified  device  (LX#  ‘/,d)  does  not  exist.  Change  LX_2_USE.\n" 
( int ) f  s_ext  )  ; 

exit  (  1 )  ; 

} 

else  { 

pr  int  f  (  "  Us  ing  device  LX#  ‘/.d...\n",  (  int  )  f  s  _ext  )  ; 

} 

/* 

*  Step  2:  format  the  filesystem  if  requested.  Note  that  formatting 

*  must  be  done  _after_  the  call  to  fs_init(). 

*  This  demo  has  compile -time  constant  of  whether  the  flash  should  be 

*  formatted  or  not. 

*/ 

rc  =  fs_init(  0,  0  ); 
if(  rc  !=  0  )  { 

printfC  "Error:  can’t  get  filesystem  initialized . \n"  ); 
exit (2)  ; 

} 

if(  MY.FORCE. FORMAT  )  { 

printfC  "Note:  File  System  Mkll  not  present. \n"  ); 
if(  0  !=  lx_f ormat  ( f s_ext  ,  0)  )  { 

printfC  "ABORT:  Can’t  Format  Mkll  File  System,  errno=‘/cd\n",  errno 
exit (2)  ; 

} 

} 

/*  Show  the  user  some  possible  solutions:  */ 
s can_f  or _ex i s t ing_f i 1 e s  ( ) ; 

/*  Prompt  for  the  file  number  to  dump  or  create.  */ 

printfC  "Which  file  number  to  affect?  "  ); 

getsC  contents  ) ; 

fnum  =  atoiC  contents  ); 

if (  fnum  <=  0  )  { 

printfC  "Error:  bad  input\n"  ); 
exit (2)  ; 

} 

printfC  "\n - \n"  ); 

do  _dump ( fnum)  ; 


} 
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